/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "canbusmaganetic.h"
#include <QDebug>
#include <QString>
#define D_RECEIVE_WAIT_COUNT_MAX 2000
#define D_RECEIVE_WAIT_SLEEP_TIME 500


CanbusMaganetic::CanbusMaganetic(QString fileNameIn, int aliasIn, int busNumberIn,
                                 int deviceIdIn, CanbusDeviceDriver *canbusDriverIn)
{
//    tcpVrep=NULL;
    isInitialedOk=false;
    alias=aliasIn;
    busNumber=busNumberIn;
    deviceId=deviceIdIn;
    canbusDriver=canbusDriverIn;
    initial(fileNameIn);

}

CanbusMaganetic::~CanbusMaganetic()
{
//    if(  tcpVrep!=NULL )
//    {
//        tcpVrep->close();
    //    }
}

int CanbusMaganetic::startPdoMode()
{
    if( false==isInitialedOk)
    {
        return -1;
    }


    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
           return 1;

            break;
        }
    case E_VENDOR_ID_FoSanXingSong:
    {
       return 1;

        break;
    }

    default:
    {
        E_NODE_MANAGE commandIn=E_NODE_MANAGE_START_PDO;
         tmpReturn=canbusDriver->manageNode(deviceId,busNumber,alias,commandIn);
         if(1==tmpReturn)
         {
             return 1;
         }
         else
         {
             return tmpReturn;
         }
         break;
    }
    }

    return -1;
}

int CanbusMaganetic::getAlias()
{
    return alias;
}

int CanbusMaganetic::sendMaganeticDataRequestCommand()
{
    QVector<unsigned char> tmpData;
    tmpData.resize(3);
    int keyReturn = canbusDriver->sendData(deviceId, busNumber, alias, tmpData);
    if(1!=keyReturn)
    {
        qDebug()<<"error, getCurrentTagData error!";
        return -1;

    }
    return 1;
}

int CanbusMaganetic::processDataFrame(QVector<bool> &sensorRawValueReturn)
{
    switch(vendorId)
    {
    case E_VENDOR_ID_FoSanXingSong:
    {
        return processDataFrame_XinSong(sensorRawValueReturn);
        break;
    }
    case E_VENDOR_ID_SHANGHAI_YUANBEN:
    {
        return processDataFrame_YuanBen(sensorRawValueReturn);
        break;
    }
    default:
    {
        qDebug()<<"CanbusMaganetic unsupport vendorid"<<vendorId;
        return -1;
    }
    }
    return -1;
}

int CanbusMaganetic::processDataFrame_XinSong(QVector<bool> &sensorRawValueReturn)
{
    QQueue<FrameData> userFrameBuffersOut;
    canbusDriver->getUserBuffer(userFrameBuffersOut);

    //process data
    int rfidFrameCount=0;
    sensorRawValueReturn.resize(16);
    for(int i=0;i<userFrameBuffersOut.size();i++)
    {
        if(userFrameBuffersOut[i].frameId==alias)
        {
            if(0==rfidFrameCount)
            {
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[0]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[1]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[2]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[3]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[4]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[5]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[6]=userFrameBuffersOut[i].data[0]&0x1;

                    sensorRawValueReturn[7]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[8]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[9]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[10]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[11]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[12]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[13]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[14]=userFrameBuffersOut[i].data[1]&0x1;

                    sensorRawValueReturn[15]=userFrameBuffersOut[i].data[2]&0x1;

//                    qDebug()<<userFrameBuffersOut[i].data[0]<<"&"<<userFrameBuffersOut[i].data[1]<<"&"
//                                              <<userFrameBuffersOut[i].data[2];
//                    qDebug()<<sensorRawValueReturn;
                    return 1;

            }




        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -1;
}

int CanbusMaganetic::processDataFrame_YuanBen(QVector<bool> &sensorRawValueReturn)
{
    QQueue<FrameData> userFrameBuffersOut;
    canbusDriver->getUserBuffer(userFrameBuffersOut);

    //process data
    int rfidFrameCount=0;
    sensorRawValueReturn.resize(16);
    for(int i=0;i<userFrameBuffersOut.size();i++)
    {
        if(userFrameBuffersOut[i].frameId==alias+0x180)//canopen协议
        {
            if(0==rfidFrameCount)
            {

                    sensorRawValueReturn[0]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[1]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[2]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[3]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[4]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[5]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[6]=userFrameBuffersOut[i].data[0]&0x1;
                    userFrameBuffersOut[i].data[0]=userFrameBuffersOut[i].data[0]>>1;
                    sensorRawValueReturn[7]=userFrameBuffersOut[i].data[0]&0x1;

                    sensorRawValueReturn[8]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[9]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[10]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[11]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[12]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[13]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[14]=userFrameBuffersOut[i].data[1]&0x1;
                    userFrameBuffersOut[i].data[1]=userFrameBuffersOut[i].data[1]>>1;
                    sensorRawValueReturn[15]=userFrameBuffersOut[i].data[1]&0x1;

                    for(int k=0;k<sensorRawValueReturn.size();k++)
                    {
                         sensorRawValueReturn[k]=!sensorRawValueReturn[k];//~按位取反,!逻辑非
                    }

//                    qDebug()<<userFrameBuffersOut[i].data[0]<<"&"<<userFrameBuffersOut[i].data[1]<<"&"
//                                              <<userFrameBuffersOut[i].data[2];
//                    qDebug()<<sensorRawValueReturn;
                    return 1;

            }




        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -1;
}

int CanbusMaganetic::getSensorRawValueFromBuffer(QVector<bool> &sensorRawValueOut)
{
    sensorRawValueOut.clear();
    for(int i=0;i<16;i++)
    {
        sensorRawValueOut.append(sensorRawValue[i]);
    }
    return 1;
}

bool CanbusMaganetic::isInitialOkStatus()
{
    return isInitialedOk;
}

int CanbusMaganetic::getSensorRawValue(QVector<bool> &sensorRawValueReturn)
{
    QVector<unsigned char> tmpData;
    tmpData.resize(3);
    int keyReturn = canbusDriver->sendData(deviceId, busNumber, alias, tmpData);
    if(1!=keyReturn)
    {
        qDebug()<<"error, getCurrentTagData error!";
        return -1;

    }
    //receive sdo data
    QVector<unsigned int> frameIdOut;
    QVector<QVector<unsigned char> > dataListOut;
    int tryCount=0;
    while(tryCount<D_RECEIVE_WAIT_COUNT_MAX)
    {
        keyReturn = canbusDriver->receiveData(deviceId, busNumber, frameIdOut, dataListOut);
        if(keyReturn>0)
        {
            break;
        }
        tryCount++;
        usleep(D_RECEIVE_WAIT_SLEEP_TIME);

    }
    if(keyReturn<=0)
    {
        qDebug()<<"error, canbus receive error!";
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1915, -1 );
        return -1;
    }

    //process data
    int rfidFrameCount=0;
    sensorRawValueReturn.resize(16);
    for(int i=0;i<frameIdOut.size();i++)
    {
        if(frameIdOut[i]==alias)
        {
            if(0==rfidFrameCount)
            {
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[0]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[1]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[2]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[3]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[4]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[5]=dataListOut[i][0]&0x1;
                    dataListOut[i][0]=dataListOut[i][0]>>1;
                    sensorRawValueReturn[6]=dataListOut[i][0]&0x1;

                    sensorRawValueReturn[7]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[8]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[9]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[10]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[11]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[12]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[13]=dataListOut[i][1]&0x1;
                    dataListOut[i][1]=dataListOut[i][1]>>1;
                    sensorRawValueReturn[14]=dataListOut[i][1]&0x1;

                    sensorRawValueReturn[15]=dataListOut[i][2]&0x1;

//                    qDebug()<<dataListOut[i][0]<<"&"<<dataListOut[i][1]<<"&"
//                                              <<dataListOut[i][2];
//                    qDebug()<<sensorRawValueReturn;
                    return 1;

            }




        }
        else
        {
//            qDebug()<<"warning, can receive unknow frameId frame!";
            continue;
        }
    }
    return -1;
}

int CanbusMaganetic::updateData()
{
    QVector<bool> tmpRawValueReturn;
 //   int returnKey=getSensorRawValue(tmpRawValueReturn);
    int returnKey=processDataFrame(tmpRawValueReturn);

    if(1==returnKey)
    {
        for(int i=0;i<tmpRawValueReturn.size();i++)
        {
            sensorRawValue[i]=tmpRawValueReturn[i];
        }
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}


#define D_SDO_TRY_MAX 5
int CanbusMaganetic::downloadSdoConfig()
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    for(int i=0;i<sdoConfigList.size();i++)
    {
        for(int k=0;k<=D_SDO_TRY_MAX;k++)
        {
            int tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,sdoConfigList[i].index
                                     ,sdoConfigList[i].subindex,sdoConfigList[i].value,sdoConfigList[i].byteLength);
            if(1==tmpReturn)
            {
                break;
            }
            else if(D_SDO_TRY_MAX==k)
            {
                qDebug()<<"downloadServoSdoConfig error,D_SDO_TRY_MAX"<<D_SDO_TRY_MAX
                       <<" alias"<<alias<<" index"<<sdoConfigList[i].index
                      <<" subindex"<<sdoConfigList[i].subindex
                     <<" byteLength"<<sdoConfigList[i].byteLength
                    <<" value"<<sdoConfigList[i].value;
                return -1;
            }
        }
    }



    return 1;
}



int CanbusMaganetic::initial(QString fileName)
{
    qDebug()<<"CanbusMaganetic initial ,file name="<<fileName;
    driverConfig=new CanbusMaganeticConfig(fileName);
    if(true==driverConfig->isInitialOkStatus())
    {
        int tmpId;
        driverConfig->getVendorId(tmpId);
        vendorId=(E_VENDOR_ID) tmpId;
        driverConfig->getSdoConfig(sdoConfigList);
        isInitialedOk=true;
        return 1;

    }
    return -1;
}

int CanbusMaganetic::getDeviceAddressInfo(int &aliasOut, int &busNumberOut, int &deviceIdOut)
{
    aliasOut=alias;
    busNumberOut=busNumber;
    deviceIdOut=deviceId;
    return 1;
}

int CanbusMaganetic::getVendorId()
{
    return vendorId;
}

